#pragma once
#include "ParticleLayer.h"
#include "BaseConstraint.h"
#include "ParticleData.h"
#include "ShapeMatchingConstraint.h"
#include "DistanceConstraint.h"
#include "VolumeConstraint.h"

enum ConstraintType
{
	DISTANCE,
	VOLUME,
	SHAPE_MATCHING
};

class CSimulationModel
{
public:
	CSimulationModel() = default;
	virtual ~CSimulationModel();
	
	CParticleData& getParticles() { return m_particles; }
	bool pumpConstraints(std::vector<CConstraint*>& voConstraints, ConstraintType vType) const;
	bool pumpConstraintGroups(std::vector<std::vector<int>>& voConstraintGroups, ConstraintType vType) const;
	
	void addSimulationParticle(const Eigen::Vector3d& vPos, const Eigen::Vector3d& vGravity);
	void generateConstraints();
	bool addShapeMatchingConstraint(int vNumberOfParticles, const std::vector<double>& vStiffness, const std::vector<int>& vParticleIndices, const std::vector<int>& vNumClusters);
	bool addDistanceConstraint(int vParticle1, int vParticle2, double vStiffness);
	bool addVolumeConstraint(const std::vector<unsigned int>& vParticles, double vStiffness);

	void setRotationMatrices(int vIndex, const Eigen::Matrix3d& vRotationMatrix);
#ifdef _UNIT_TEST
	std::vector<Eigen::Matrix3d> getRotationMatrices()
	{
		return m_RotationMatrices;
	}
#endif // _UNIT_TEST

private:
	void __clear();
	template<class T>
	void __initConstraintGroups(const std::vector<T*>& vConstraints, std::vector<std::vector<int>>& voConstraintGroups);

	std::vector<CShapeMatchingConstraint*> m_ShapeMatchingConstraints;
	std::vector<std::vector<int>> m_ShapeMatchingConstraintGroups;
	std::vector<CDistanceConstraint*> m_DistanceConstraints;
	std::vector<std::vector<int>> m_DistanceConstraintGroups;
	std::vector<CVolumeConstraint*> m_VolumeConstraints;
	std::vector<std::vector<int>> m_VolumeConstraintGroups;

	CParticleData m_particles;
	std::vector<Eigen::Matrix3d> m_RotationMatrices;
};

template<class T>
void CSimulationModel::__initConstraintGroups(const std::vector<T*>& vConstraints, std::vector<std::vector<int>>& voConstraintGroups)
{
	int particleCount = m_particles.getSize();
	int ConstraintCount = vConstraints.size();
	std::vector<unsigned char*> mapping;
	for (int i = 0; i < ConstraintCount; i++)
	{
		const T* pConstraint = vConstraints[i];

		bool addToNewGroup = true;
		for (int j = 0; j < voConstraintGroups.size(); j++)
		{
			bool addToThisGroup = true;
			for (int k = 0; k < pConstraint->getNumOfParticles(); k++)
			{
				int ParticleIndice = pConstraint->getParticleIndice(k);
				if (mapping[j][ParticleIndice] != 0)
				{
					addToThisGroup = false;
					break;
				}
			}

			if (addToThisGroup)
			{
				voConstraintGroups[j].push_back(i);
				for (int k = 0; k < pConstraint->getNumOfParticles(); k++)
				{
					mapping[j][pConstraint->getParticleIndice(k)] = 1;
				}

				addToNewGroup = false;
				break;
			}
		}

		if (addToNewGroup)
		{
			mapping.push_back(new unsigned char[particleCount]);
			memset(mapping[mapping.size() - 1], 0, sizeof(unsigned char)*particleCount);
			std::vector<int> newGroup;
			newGroup.push_back(i);
			voConstraintGroups.push_back(newGroup);
			for (int k = 0; k < pConstraint->getNumOfParticles(); k++)
				mapping[voConstraintGroups.size() - 1][pConstraint->getParticleIndice(k)] = 1;
		}
	}

	for (int i = 0; i < mapping.size(); i++)
	{
		delete[] mapping[i];
	}
	mapping.clear();
}